#ifndef RGBD_KINECT2_H
#define RGBD_KINECT2_H

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <libfreenect2/libfreenect2.hpp>
#include <libfreenect2/registration.h>
#include <libfreenect2/frame_listener_impl.h>

class RGBD_Kinect2
{

        libfreenect2::Freenect2Device::IrCameraParams depth_param;

        libfreenect2::Freenect2 freenect2;
        libfreenect2::Freenect2Device *dev = 0;
        libfreenect2::PacketPipeline *pipeline = 0;

        libfreenect2::SyncMultiFrameListener *listener;
        libfreenect2::FrameMap *frames;

        libfreenect2::Registration *registration;
        libfreenect2::Frame *undistorted;
        libfreenect2::Frame *registered;

        int convertToXYZ(const libfreenect2::Frame *undistorted, pcl::PointCloud<pcl::PointXYZ> *XYZ_vect,
                                         float limit_near, float limit_far);
        // RGBX: uint8*4, X = 255
        int convertToXYZRGB(const libfreenect2::Frame *undistorted,
                                                const libfreenect2::Frame *registered, pcl::PointCloud<pcl::PointXYZRGB> *XYZRGB_vect,
                                                float limit_near, float limit_far);

        bool fetchAndLockFrame();

public:
        size_t framecount = 0;
        bool Failed = false;
        struct BoundingBox{
                float x_min, x_max;
                float y_min, y_max;
                float z_min, z_max;

                void clear();
                void setP(float x, float y, float z);
        } bbox;

public:
//	explicit RGBD_Kinect2(int edge_clip_bottom = 0, int edge_clip_top = 0,
//						  int edge_clip_left = 0, int edge_clip_right = 0);
    RGBD_Kinect2();
    void init();
        ~RGBD_Kinect2();

        int fetchXYZ(pcl::PointCloud<pcl::PointXYZ> *XYZ, float limit_near = 0.01, float limit_far = 8.0);
        int fetchXYZRGB(pcl::PointCloud<pcl::PointXYZRGB> *XYZRGB, float limit_near = 0.01, float limit_far = 8.0);
        int fetchColorImage(cv::Mat &image);
        int fetchHdColorImage(cv::Mat &image);
        int fetchDepthImage(cv::Mat &image);
private:
        cv::Mat color_image;
        cv::Mat depth_image;
        cv::Mat color_hd_image;
};


#endif // RGBD_KINECT2_H
